
#include <AFMotor.h>
#include <SoftwareSerial.h>

// Set up a new SoftwareSerial object with RX in digital pin 10 and TX in digital pin 9

SoftwareSerial mySerial(10, 9);
AF_DCMotor motor1(1);
AF_DCMotor motor2(2);

AF_DCMotor motor[2]={motor1,motor2};
#define MOTORNUM  2
//speed 小于120时，启动不了
void forward_drive(int speed)
{
for(char i=0;i<MOTORNUM;i++)
  {
     motor[i].run(FORWARD);
     motor[i].setSpeed(150); 
   // motor[i].run(RELEASE);

  } 
  delay(30);
 for(char i=0;i<MOTORNUM;i++)
  {
     motor[i].run(FORWARD);
     motor[i].setSpeed(speed); 
   // motor[i].run(RELEASE);

  }
}
void left_drive(int speed1,int speed2)
{
    motor[0].run(FORWARD);
    motor[0].setSpeed(speed1); 
  // motor[i].run(RELEASE);
    motor[1].run(FORWARD); 
    motor[1].setSpeed(speed2); 
  


}
void right_drive(int speed1,int speed2)
{
    motor[0].run(FORWARD);
    motor[0].setSpeed(speed2); 
  // motor[i].run(RELEASE);
    motor[1].run(FORWARD); 
    motor[1].setSpeed(speed1); 
 


}
void backward_drive(int speed)
{
  for(char i=0;i<MOTORNUM;i++)
  {
     motor[i].run(BACKWARD);
      motor[i].setSpeed(120); 
   // motor[i].run(RELEASE);

  }
  delay(30);

for(char i=0;i<MOTORNUM;i++)
  {
     motor[i].run(BACKWARD);
      motor[i].setSpeed(speed); 
   // motor[i].run(RELEASE);

  }

}
void rotate(int rot)
{
   char dir=0;
   char dir_1=0;
  if(rot>0)
  {
    dir=BACKWARD;
    dir_1=FORWARD;


  }
  else
  {
    dir=FORWARD;
    dir_1=BACKWARD;

  }
 
  for(char i=0;i<MOTORNUM;i++)
  {
      if(i==0||i==2)
      {
        motor[i].run(dir);
        motor[i].setSpeed(rot); 
      }
      if(i==1||i==3)
      {
        motor[i].run(dir_1);
        motor[i].setSpeed(rot); 
      }
   // motor[i].run(RELEASE);
  }

  
}
void stop()
{
  for(char i=0;i<MOTORNUM;i++)
  {
    motor[i].setSpeed(0); 
    motor[i].run(RELEASE);

  }

}
 
int trigPin = 15;    // Trigger
int echoPin = 14;    // Echo
long duration, cm, inches;
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);           // set up Serial library at 9600 bps
  Serial.println("Motor test!");
  mySerial.begin(9600);
  // turn on motor
 for(char i=0;i<MOTORNUM;i++)
  {
    motor[i].setSpeed(200); 
    motor[i].run(RELEASE);

  }
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
 
 
}
char c;
void loop() {
  // put your main code here, to run repeatedly:
//前进
  if(mySerial.available()>0)
    {
      c = mySerial.read();
      Serial.println(c);
    }
    if(c=='1')
    {
      forward_drive(80);
    }
    if(c=='2')
    {
      backward_drive(80);
    }
   if(c=='0')
    {
      stop();
    }

    digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  pinMode(echoPin, INPUT);
  duration = pulseIn(echoPin, HIGH);
  int distance = (duration*.0343)/2;
  if(distance<10||distance>100)
    stop();
  if(distance>10&&distance<20)
    forward_drive(60);
  //step2. back 1m

}
